package com.ats.audiotoscore;

import java.util.HashMap;
import java.util.Arrays;
//import android.util.Log;

public class PitchDetection {
//	private static final String TAG = "AudioToScore";
	private double samples2[] = null;
	private Peak peaks[];
	private int maxPeaks = 16;
	private HashMap<Double, String> notesStr;
	private HashMap<Double, Integer> notes;
	private double prevPitch = 0;
	private double detectedPitch = 0;
	private int pitchConfidence = 0;
	private int maxConfidence = 3;
	private double acceptedError = 0.04;

	public PitchDetection(int accuracy) {
		switch (accuracy) {
		case 1: maxConfidence = 2; break;
		case 2: maxConfidence = 1; break;
		default:
			maxConfidence = 3; break;
		}

		peaks = new Peak[maxPeaks];
		for (int i=0; i<peaks.length; i++) {
			peaks[i] = new Peak();
		}

		BuildNotesArray(440);
	}

	private void BuildNotesArray(double baseHz) {
		double mult = 1.0594630943592952645618252949463;
		double hz;

		notesStr = new HashMap<Double, String>();
		hz = baseHz / 16;
		notesStr.put(hz, "A0");
		hz = hz * mult;
		notesStr.put(hz, "A#0");
		hz = hz * mult;
		notesStr.put(hz, "B0");
		hz = hz * mult;
		for (int i=1; i<10; i++) {
			notesStr.put(hz, "C"+i);
			hz = hz * mult;
			notesStr.put(hz, "C#"+i);
			hz = hz * mult;
			notesStr.put(hz, "D"+i);
			hz = hz * mult;
			notesStr.put(hz, "D#"+i);
			hz = hz * mult;
			notesStr.put(hz, "E"+i);
			hz = hz * mult;
			notesStr.put(hz, "F"+i);
			hz = hz * mult;
			notesStr.put(hz, "F#"+i);
			hz = hz * mult;
			notesStr.put(hz, "G"+i);
			hz = hz * mult;
			notesStr.put(hz, "G#"+i);
			hz = hz * mult;
			notesStr.put(hz, "A"+i);
			hz = hz * mult;
			notesStr.put(hz, "A#"+i);
			hz = hz * mult;
			notesStr.put(hz, "B"+i);
			hz = hz * mult;
		}

		notes = new HashMap<Double, Integer>();
		hz = baseHz / 16;
		int midiNote = 21;
		for (int i=0; i<10; i++) {
			for (int n=0; n<12; n++) {
				notes.put(hz, midiNote++);
				hz = hz * mult;
			}
		}
	}
	
	public String HzToNote(double hz) {
		double bestDistanceFoundYet = Double.MAX_VALUE;
		String str = "";

		if (hz == 0)
			return "";

		for (HashMap.Entry<Double, String> entry : notesStr.entrySet()) {
		    Double key = entry.getKey();
		    String value = entry.getValue();

		    if (hz == key) {
				return value;
			} else {
				double d = Math.abs(hz - key);
			    if (d < bestDistanceFoundYet) {
			    	bestDistanceFoundYet = d;
			    	str = value;
			    }
			}
		}
		return str;
	}

	public int HzToMidiNote(double hz) {
		double bestDistanceFoundYet = Double.MAX_VALUE;
		int ret = 0;

		if (hz == 0)
			return 0;

		for (HashMap.Entry<Double, Integer> entry : notes.entrySet()) {
		    Double key = entry.getKey();
		    Integer value = entry.getValue();

		    if (hz == key) {
				return value;
			} else {
				double d = Math.abs(hz - key);
			    if (d < bestDistanceFoundYet) {
			    	bestDistanceFoundYet = d;
			    	ret = value;
			    }
			}
		}
		return ret;
	}

	public double[] GetFFTOutput() {
		return samples2;
	}

	public int GetBufferLength() {
		return samples2 != null ? samples2.length : 0;
	}

	private void ResetPeaks() {
		for (int i=0; i<peaks.length; i++) {
			peaks[i].db = 0;
			peaks[i].hz = 99999;
			peaks[i].index = 0;
		}
	}

	private void AddPeak(int pk, int index, double db, double hz) {
		if (pk >= peaks.length)
			return;

		peaks[pk].db = db;
		peaks[pk].hz = hz;
		peaks[pk].index = index;
	}

	private void DetectPeaks(int sampleRate, double rmsdb) {
		double pdb = 0;
		double lowdb = 0;
		double highdb = 0;
		int highidx = 0;
		int status = 0;
		int pk = 0;

		for (int i=1; i<samples2.length/2; i++) {
			double db = samples2[i];

			if (db > pdb) {
				if (status == 2) {
					if (highdb > (lowdb+10)) {
						AddPeak(pk++, highidx, highdb, (highidx * sampleRate) / samples2.length);
					}
				}

				if (status != 1) {
					status = 1;
				}
				highdb = db;
				highidx = i;
			} else {
				status = 2;
				lowdb = db;
			}

			pdb = db;
		}
	}

	public Peak[] GetPeaks() { return peaks; }
	public double GetDetectedPitch() { return detectedPitch; } 
	public int GetPitchConfidence() { return pitchConfidence; }

	public int GetDetectedNote() {
		return HzToMidiNote(GetDetectedPitch());
	}

	public boolean IsNoteSafe() {
		return GetPitchConfidence() >= maxConfidence;
	}

	private void ComputeDetectedPicth(double pitch) {
		if (pitch == 0) {
			detectedPitch = 0;
			prevPitch = 0;
			pitchConfidence = 0;
			return;
		}

//		Log.i(TAG, "ComputeDetectedPitch "+pitch);
		if (maxConfidence == 1) {
			detectedPitch = pitch;
			pitchConfidence = 1;
			return;
		}

		double estimatedPitch = -1;

		if (prevPitch == 0) {
			estimatedPitch = pitch;
			prevPitch = pitch;
			pitchConfidence = 1;
		} else if ((Math.abs(prevPitch - pitch) / pitch) < acceptedError) {
			// similar : remember and increment pitch
			estimatedPitch = pitch;
			prevPitch = pitch;
			pitchConfidence = Math.min(pitchConfidence+1, maxConfidence);
		} else if ((pitchConfidence >= maxConfidence-2) && ((Math.abs(prevPitch - 2.*pitch) / (2.*pitch)) < acceptedError)) {
			// close to half the last pitch, which is trusted
			estimatedPitch = 2.*pitch;
			prevPitch = estimatedPitch;
			pitchConfidence = Math.min(pitchConfidence+1, maxConfidence);
		} else if ((pitchConfidence >= maxConfidence-2) && ((Math.abs(prevPitch - 0.5*pitch) / (0.5*pitch)) < acceptedError)) {
			// close to twice the last pitch, which is trusted
			estimatedPitch = 0.5*pitch;
			prevPitch = estimatedPitch;
			pitchConfidence = Math.min(pitchConfidence+1, maxConfidence);
		} else {
			// nothing like this : very different value
			if (pitchConfidence >= maxConfidence-2) {
				// previous trusted : keep previous
				estimatedPitch = prevPitch;
				pitchConfidence = Math.max(pitchConfidence-1, 0);
			} else {
				// previous not trusted : take current
				estimatedPitch = pitch;
				prevPitch = pitch;
				pitchConfidence = 1;
			}
		}
	
		if (pitchConfidence >= 1) {
			detectedPitch = estimatedPitch;
		} else {
			detectedPitch = 0;
		}
	}

	private boolean TestHZ(double hz1, double hz2) {
		return (Math.abs(hz1 - hz2) / hz2) < acceptedError;
	}

	private int FindPeak(int i, double hz) {
		for (; i<peaks.length; i++) {
			if (TestHZ(hz, peaks[i].hz))
				return i;
		}

		return 0;
	}

	private boolean FindHarmonics(int i, double db, double hz0, double hz1) {
		i = FindPeak(i+1, hz0);
		if (i != 0 && peaks[i].db / db >= 0.7) {
			i = FindPeak(i+1, hz1);
			if (i != 0 && peaks[i].db / db >= 0.7) {
				return true;
			}
		}

		return false;
	}

	private double ProcessHarmonics() {
		int peakCount = 0;
		for (int i=0; i<peaks.length && peaks[i].db != 0; i++) {
			peakCount++;
		}

		Arrays.sort(peaks, Peak.ComparatorByDB);
		double hz = peaks[0].hz;
		double db = peaks[0].db;
		if (hz == 99999)
			return 0;

		Arrays.sort(peaks, Peak.ComparatorByHZ);
		if (peakCount >= 3 && hz != peaks[0].hz) {
/*			for (int i=0; i<peakCount; i++) {
				Log.i(TAG, "hz["+i+"]="+HzToNote(peaks[i].hz)+" ("+peaks[i].hz+") db="+peaks[i].db);
			}*/

			for (int i=0; i<peakCount-3; i++) {
				double phz = peaks[i].hz;
				if (FindHarmonics(i, db, (phz/2)*3, (phz/2)*4)) {
//					Log.i(TAG, "found phz/2");
					return phz/2;
				}

				if (FindHarmonics(i, db, phz*2, phz*3)) {
//					Log.i(TAG, "found phz");
					return phz;
				}
			}
		}

		return hz;
	}

	public void UpdateSamples(double samples[], int sampleRate, double rmsdb) {
		if (samples2 == null || samples.length != samples2.length) {
			samples2 = new double[samples.length];
		}
		for (int i=0; i<samples2.length; i++) {
			samples2[i] = 0;
		}
		Fft.transform(samples, samples2);
		for (int i=1; i<samples.length/2; i++) {
			double amp = Math.sqrt(Math.pow(samples[i], 2) + Math.pow(samples2[i], 2));
			samples2[i] = 10 * Math.log10(amp);
		}

		ResetPeaks();

		if (rmsdb >= 25) {
			DetectPeaks(sampleRate, rmsdb);
			ComputeDetectedPicth(ProcessHarmonics());
		} else {
			ComputeDetectedPicth(0);
		}
	}
}
